﻿using ServoDrivesLib.Controls;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace ServoDrivesLib.Device
{
    public abstract class DeviceControl
    {
        protected readonly CardControl? servo;
        public int AxisId = 0;
        public double Deviation = 0.02;
        public double Timeout = 5000;
        public DeviceControl(CardControl? servo)
        {
            this.servo = servo;
        }

        public virtual int Home()
        {
            return (servo?.ServoControl?.SetHomeMove(AxisId)).Value;
        }
        public virtual int SetProFile(double unit, double maxSpeed, double tAcc, double tDec, double sramp)
        {
            servo?.SetEquiv(AxisId, unit);
            var ret = servo?.SetProFile(AxisId, 0, maxSpeed, tAcc, tDec, 0, sramp);
            return ret.Value;
        }

        public virtual int Move(double pos, bool en)
        {
            int ret = 0;
            this.servo?.MoveAxisAbs(AxisId, pos);
            if (en)
            {
                double timeout = DateTime.Now.TimeOfDay.TotalMilliseconds + Timeout;
                while (DateTime.Now.TimeOfDay.TotalMilliseconds - timeout > 0)
                {
                    int status = 0;
                    double p = 0;
                    status = servo?.GetAxisMoveStatus(AxisId) ?? 1;
                    servo?.GetEquiv(AxisId, ref p);
                    if (status == 0 && Math.Abs(pos - p) < Deviation)
                    {
                        ret = 1;
                        break;
                    }
                }
            }
            return ret;
        }

        public virtual int Enable(bool en)
        {
            int ret = 0;
            if (en)
            {
                this.servo?.SetAxisEnable(AxisId);
                double timeout = DateTime.Now.TimeOfDay.TotalMilliseconds + Timeout;
                while (DateTime.Now.TimeOfDay.TotalMilliseconds - timeout > 0)
                {
                    int status = 0;
                    double p = 0;
                    servo?.GetAxisStatus(AxisId, ref status);
                    if (status == 0x04)
                    {
                        ret = 1;
                        break;
                    }
                    else
                    {
                        ret = -1;
                    }
                }
            }
            else
            {
                this.servo?.SetAxisDisable(AxisId);
            }

            return ret;
        }

        public virtual void ClearAxisError()
        {
            this.servo?.ClearAxisError(AxisId);
        }

        public virtual int GetAxisError()
        {
            int error = 0;
            this.servo?.GetAxisError(AxisId, ref error);

            return error;
        }
    }
}
